#ifndef SRC_UTILS_RICHPOINT_HPP_
#define SRC_UTILS_RICHPOINT_HPP_

#include <Eigen/Core>      // eigen
#include <Eigen/StdVector> // eigen
#include <algorithm>
#include <cstdint>
#include <list>
#include <memory>
#include <stdexcept>
#include <vector>

/**
 * @brief 点的坐标
 *
 */
struct PointCoord
{
  int16_t row;
  int16_t col;
};

struct PixelCoord
{
  PixelCoord() : row(0), col(0) {}
  PixelCoord(int16_t row_, int16_t col_) : row(row_), col(col_) {}
  PixelCoord operator+(const PixelCoord &other) const
  {
    return PixelCoord(row + other.row, col + other.col);
  }

  int16_t row;
  int16_t col;
};

/**
 * @brief RichPoint点类
 *
 */
class RichPoint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // Align排列 vector<RichPoint>
  using AlignedVector =
      std::vector<RichPoint, Eigen::aligned_allocator<RichPoint>>;

  RichPoint() {}
  explicit RichPoint(float x, float y, float z) : _point(x, y, z) {}                                                                     // explicit翻译为明确的，表明不能隐式地构造
  explicit RichPoint(float x, float y, float z, uint16_t ring, float intensity) : _point{x, y, z}, _ring{ring}, _intensity{intensity} {} // 自己添加强度
  explicit RichPoint(Eigen::Vector3f &eigen_vec) : _point(eigen_vec) {}
  ~RichPoint() {}

  inline int ring() const { return _ring; }
  // 作右值
  inline float intensity() const { return _intensity; }
  // inline int intensity() { return _intensity; }
  inline float x() const { return _point.x(); }
  inline float y() const { return _point.y(); }
  inline float z() const { return _point.z(); }
  // 自己添加点的强度
  inline float &intensity() { return _intensity; }
  inline uint16_t &ring() { return _ring; }
  inline float &x() { return _point.x(); }
  inline float &y() { return _point.y(); }
  inline float &z() { return _point.z(); }

  inline const Eigen::Vector3f &AsEigenVector() const { return _point; }
  inline Eigen::Vector3f &AsEigenVector() { return _point; }

  inline float DistToSensor2D() const
  {
    return sqrt(_point.x() * _point.x() + _point.y() * _point.y());
  }

  inline float DistToSensor3D() const
  {
    return sqrt(_point.x() * _point.x() + _point.y() * _point.y() +
                _point.z() * _point.z());
  }

  inline RichPoint &operator=(const RichPoint &other)
  {
    if (this != &other)
    { // self-assignment check expected
      _point = other.AsEigenVector();
      _ring = other.ring();
    }
    return *this;
  }

  inline RichPoint &operator=(const Eigen::Vector3f &other)
  {
    this->_point = other;
    return *this;
  }

  inline bool operator==(const RichPoint &other) const
  {
    return this->x() == other.x() && this->y() == other.y() &&
           this->z() == other.z() && this->ring() == other.ring();
  }

private:
  Eigen::Vector3f _point = Eigen::Vector3f::Zero();
  uint16_t _ring = 0;
  float _intensity = 0;
};

#endif // SRC_UTILS_RADIANS_H_
